#include "MagnetObject.h"
//#include <vector>

//using namespace std;

#ifndef PI
#define PI 3.14159265359
#endif


const float MagnetObject::k = 10000.0f;
const Vector3 MagnetObject::maxrange = Vector3(15, 4, 15);
const float MagnetObject::inverseU0 = 1.0f / (MagnetObject::k * 4 * PI);

Vector3 MagnetObject::calMfield(Vector3 wpos, bool Interpolation){

	
	if(!Interpolation)
		return calMfield(wpos);


	Vector3 localpos;
	localpos = wpos - pos;
	
	Vector3 retval(0,0,0);
	Vector3 temp = localpos;
	temp = temp.absSelf();
	
	if(temp.x() >= maxrange.x() || temp.z() >= maxrange.z() || temp.y() >= maxrange.y())
		return Vector3(0,0,0);

	localpos *= scale;
	int x = floor(localpos.x());
	int y = floor(localpos.y());
	int z = floor(localpos.z());
	
	float ax, bx;

	ax = localpos.x() - x;
	bx = 1- ax;

	temp = Mfield[(int)(x+maxrange.x()*scale)][(int)(y+maxrange.y()*scale)][(int)(z+maxrange.z()*scale)] * bx + Mfield[(int)(x+1 + scale*maxrange.x())][(int)(y+maxrange.y()*scale)][(int)(z+maxrange.z()*scale)] * ax;
	Vector3 temp2 = Mfield[(int)(x+maxrange.x()*scale)][(int)(y+maxrange.y()*scale)][(int)(z+1 + maxrange.z()*scale)] * bx + Mfield[(int)(x+1 + maxrange.x()*scale)][(int)(y+maxrange.y()*scale)][(int)(z+1 + maxrange.z()*scale)] * ax;
	
	Vector3 temp3 = Mfield[(int)(x+maxrange.x()*scale)][(int)(y+1+maxrange.y()*scale)][(int)(z+maxrange.z()*scale)] * bx + Mfield[(int)(x+1 + scale*maxrange.x())][(int)(y+1+maxrange.y()*scale)][(int)(z+maxrange.z()*scale)] * ax;
	Vector3 temp4 = Mfield[(int)(x+maxrange.x()*scale)][(int)(y+1+maxrange.y()*scale)][(int)(z+1 + maxrange.z()*scale)] * bx + Mfield[(int)(x+1 + maxrange.x()*scale)][(int)(y+1+maxrange.y()*scale)][(int)(z+1 + maxrange.z()*scale)] * ax;
	
	
	
	ax = localpos.y() - y;
	bx = 1-ax;

	temp = temp * bx + temp3 * ax;
	temp2 = temp2 * bx + temp4 * ax;

	
	ax = localpos.z() - z;
	bx = 1-ax;

	retval = temp * bx + temp2 * ax;

	return retval;

	
}//Vector3 calMfield(pos);

Vector3 MagnetObject::calMfield(Vector3 wpos){

	
	Vector3 origin;
	Vector3 mmi;
	Vector3 nor;
	Vector3 Bfield;
	for(auto mit = vMagnet.begin(); mit != vMagnet.end(); ++mit){

		origin = (*mit)->pos;
		mmi = (*mit)->mm;

		nor = wpos - origin;
		float dist = nor.length();
		nor.normalize();
		Bfield += (3*nor*(nor.dot(mmi)) - mmi)*pow(dist,-3);


	}//for(magnet)

	Bfield *= MagnetObject::k;

	return Bfield;


}//Vector3 calMfield(wpos, Interpolation)


Vector3 MagnetObject::ComputeForce(Vector3 wpos, Vector3 mmk){

	Vector3 origin;
	Vector3 mmi;
	Vector3 nor;
	float dist;
	Vector3 temp;
	Vector3 retval;

	for(auto mit = vMagnet.begin(); mit != vMagnet.end(); ++mit){


		origin = (*mit)->pos;
		mmi = (*mit)->mm;

		nor = wpos - origin;
		dist = nor.length();
		nor.normalize();

		temp = -15*nor * (mmk.dot(nor) * mmi.dot(nor)) + 3 * nor * mmk.dot(mmi) + 3 * (mmk * mmi.dot(nor) + mmi * mmk.dot(nor));
		retval += temp * pow(dist, -4);


	}//for all magnet cell



	return retval * MagnetObject::k;

}//Vector3 ComputeForce(wpos)

Vector3 MagnetObject::CalPForce(Vector3 wpos){

	
	Vector3 localpos;
	localpos = wpos - pos;

	Vector3 retval(0, 0,0);
	Vector3 temp = localpos;
	temp = temp.absSelf();

	if(temp.x() >= maxrange.x() || temp.y() >= maxrange.y() || temp.z() >= maxrange.z())
		return Vector3(0,0,0);


	localpos *= scale;
	int x = floor(localpos.x());
	int y = floor(localpos.y());
	int z = floor(localpos.z());

	float ax, bx;

	ax = localpos.x() - x;
	bx = 1- ax;

	temp = Ffield[(int)(x+maxrange.x()*scale)][(int)(y+maxrange.y()*scale)][(int)(z+maxrange.z()*scale)] * bx + Ffield[(int)(x+1 + scale*maxrange.x())][(int)(y+maxrange.y()*scale)][(int)(z+maxrange.z()*scale)] * ax;
	Vector3 temp2 = Ffield[(int)(x+maxrange.x()*scale)][(int)(y+maxrange.y()*scale)][(int)(z+1 + maxrange.z()*scale)] * bx + Ffield[(int)(x+1 + maxrange.x()*scale)][(int)(y+maxrange.y()*scale)][(int)(z+1 + maxrange.z()*scale)] * ax;
	
	Vector3 temp3 = Ffield[(int)(x+maxrange.x()*scale)][(int)(y+1+maxrange.y()*scale)][(int)(z+maxrange.z()*scale)] * bx + Ffield[(int)(x+1 + scale*maxrange.x())][(int)(y+1+maxrange.y()*scale)][(int)(z+maxrange.z()*scale)] * ax;
	Vector3 temp4 = Ffield[(int)(x+maxrange.x()*scale)][(int)(y+1+maxrange.y()*scale)][(int)(z+1 + maxrange.z()*scale)] * bx + Ffield[(int)(x+1 + maxrange.x()*scale)][(int)(y+1+maxrange.y()*scale)][(int)(z+1 + maxrange.z()*scale)] * ax;
	
	
	
	ax = localpos.y() - y;
	bx = 1-ax;

	temp = temp * bx + temp3 * ax;
	temp2 = temp2 * bx + temp4 * ax;

	
	ax = localpos.z() - z;
	bx = 1-ax;

	retval = temp * bx + temp2 * ax;

	
	
	return retval;


	
}//Vector3 calPForce();

void MagnetObject::SetMfield(){

	Vector3 wpos;
	Vector3 temp;

	float dx, dy, dz;
	
	for(int x = -maxrange.x()*scale; x<=maxrange.x()*scale; x++)
		for(int y = -maxrange.y() * scale; y<=maxrange.y()*scale; y++)
		for(int z = -maxrange.z()*scale; z<=maxrange.z()*scale; z++){
			
			dx = x/(float)scale;
			dy = y/(float)scale;
			dz = z/(float)scale;

			wpos = pos + Vector3(dx, dy, dz);
			
			temp = calMfield(wpos);
			Mfield[(int)(x+maxrange.x()*scale)][(int)(y+maxrange.y()*scale)][(int)(z+maxrange.z()*scale)] = temp;

			
		}//for(xyz)

}//void SetMfield

void MagnetObject::SetFfield(){


	Vector3 wpos;
	Vector3 mm;
	float dx, dy, dz;

	for(int x = -maxrange.x() * scale ; x<=maxrange.x() * scale; x++)
		for(int y = -maxrange.y() * scale; y<=maxrange.y() * scale; y++)
			for(int z = -maxrange.z() * scale; z<=maxrange.z() * scale; z++){

				dx = x/(float)scale;
				dy = y/(float)scale;
				dz = z/(float)scale;

				wpos = pos + Vector3(dx, dy, dz);
				mm = Mfield[(int)(x+maxrange.x()*scale)][(int)(y+maxrange.y()*scale)][(int)(z+maxrange.z()*scale)];
				mm *= 3 * 0.999 * inverseU0 * volParticle;
				Ffield[(int)(x+maxrange.x()*scale)][(int)(y+maxrange.y()*scale)][(int)(z+maxrange.z()*scale)] = ComputeForce(wpos, mm);
			}//for(xyz)


}//void SetFfield